﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Globalization;
using System.Text;
using System.Windows.Forms;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.ServiceModel.Dssp;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using sonar = Microsoft.Robotics.Services.Sonar.Proxy;
using contact = Microsoft.Robotics.Services.ContactSensor.Proxy;


namespace Diversity.Robotics.Navigation.GridSlamService
{
    partial class GridSlamForm : Form
    {
        GridSlamFormEvents _eventsPort;

        public GridSlamForm(GridSlamFormEvents eventsPort)
        {
            _eventsPort = eventsPort;
            InitializeComponent();
        }

        #region Load / Close / Update
        private void GridSlamForm_Load(object sender, EventArgs e)
        {
            _eventsPort.Post(new OnLoad(this));
          
        }

        private void GridSlamForm_Closed(object sender, FormClosedEventArgs e)
        {
            _eventsPort.Post(new OnClosed(this));
        }

        public void UpdateFromState(GridSlamState state)
        {
            // this lets the datagrids display the state
            // edits are sent directly back to state - naughty but nice
            ReplaceBeamModels(state.BeamModelStates);
            ReplaceConeModels(state.ConeModelStates);
            ReplaceSensorTypes(state.SensorTypes);
            ReplaceSensors(state.RobotState.Sensors);
        }
        #endregion

        #region Directory Lookups

        private void buttonConnectToNode_Click(object sender, EventArgs e)
        {

            string machine = textBoxMachine.Text;

            if (machine.Length == 0)
            {
                textBoxMachine.Focus();
                return;
            }

            ushort port = 0;
            if (ushort.TryParse(textBoxPort.Text, out port)) {}

            UriBuilder builder = new UriBuilder(Schemes.DsspTcp, machine, port, ServicePaths.InstanceDirectory);

            _eventsPort.Post(new OnConnect(this, builder.ToString()));
        }
       

        public void ReplaceServicesList(ServiceInfoType[] list)
        {
            bool subscribedToDrive = false;

            List<string> interestingServices = new List<string>();
            interestingServices.Add(drive.Contract.Identifier);
            interestingServices.Add(sonar.Contract.Identifier);
            interestingServices.Add(contact.Contract.Identifier);
            interestingServices.Add(lrf.Contract.Identifier);

            listBoxServices.BeginUpdate();
            try
            {
                listBoxServices.Tag = list;
                listBoxServices.Items.Clear();

                if (list.Length > 0)
                {
                    UriBuilder node = new UriBuilder(list[0].Service);
                    node.Path = null;
                 //   lblNode.Text = node.Host + ":" + node.Port;
                 //   txtMachine.Text = node.Host;
                 //   txtPort.Text = node.Port.ToString();

                 //   linkDirectory.Enabled = true;
                }
                else
                {
                  //  lblNode.Text = string.Empty;
                  //  linkDirectory.Enabled = false;
                }

                foreach (ServiceInfoType info in list)
                {
                    if (interestingServices.Contains(info.Contract))
                    {
                        string prefix = string.Empty;
                        foreach (PartnerType partner in info.PartnerList)
                        {
                            if (partner.Name.ToString().EndsWith(":Entity"))
                            {
                                prefix = partner.Service;
                                int last = prefix.LastIndexOf('/');
                                prefix = "(" + prefix.Substring(last + 1) + ")";
                                break;
                            }
                        }
                        Uri serviceUri = new Uri(info.Service);
                        listBoxServices.Items.Add(prefix + " " + serviceUri.AbsolutePath);
                        Console.Out.WriteLine("Service is " + info.Service);
                        if (checkBoxAutoConnectServices.Checked == true)
                        {
                           
                            // wire in the services
                            switch (info.Contract)
                            {
                                case drive.Contract.Identifier :
                                    // connect a drive
                                    if (!subscribedToDrive)
                                    {
                                        Console.Out.WriteLine("Sending drive connect");
                                        subscribedToDrive = true;
                                        _eventsPort.Post(new OnConnectDrive(this, info.Service));
                                    } else
                                    {
                                        Console.Out.WriteLine("Already subscribed to drive");
                                    }
                                    break;
                                case sonar.Contract.Identifier :
                                    // connect a sonar
                                    break;
                                case lrf.Contract.Identifier :
                                    Console.Out.WriteLine("Sending lrf connect");
                                    _eventsPort.Post(new OnConnectLRF(this, info.Service));
                                    // connect a sick
                                    break;
                                case contact.Contract.Identifier :
                                    // connect a bumper
                                    break;
                                default:
                                    break;
                            }
                        }
                    }
                }


                // btnConnect_ArticulatedArm.Enabled = ServiceByContract(arm.Contract.Identifier) != null;
                //  btnJointParamsApply.Enabled = btnConnect_ArticulatedArm.Enabled;
            }
            finally
            {
                listBoxServices.EndUpdate();

                // enable the timer to view updates
                timerMapUpdate.Enabled = true;
            }
        }

        ServiceInfoType FindServiceInfoFromServicePath(string path)
        {
            ServiceInfoType[] list = (ServiceInfoType[])listBoxServices.Tag;

            UriBuilder builder = new UriBuilder(list[0].Service);
            builder.Path = path;

            string uri = builder.ToString();

            return FindServiceInfoFromServiceUri(uri);
        }

        ServiceInfoType FindServiceInfoFromServiceUri(string uri)
        {
            ServiceInfoType[] list = (ServiceInfoType[])listBoxServices.Tag;

            foreach (ServiceInfoType si in list)
            {
                if (si.Service == uri)
                    return si;
            }
            return null;
        }

        #endregion

        #region Menu Items
        private void exitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            // exit
        }

        private void viewDirectoryToolStripMenuItem_Click(object sender, EventArgs e)
        {
            // launch directory
        }

        private void aboutToolStripMenuItem_Click(object sender, EventArgs e)
        {
            // show about form
        }
        #endregion

        #region Motion Tab
        private void buttonRobotSave_Click(object sender, EventArgs e)
        {
            // save robot params
        }

        private void buttonOdometryModelSave_Click(object sender, EventArgs e)
        {
            // save odometry params
            double vErr1, vErr2, rErr1, rErr2;
            if (double.TryParse(textBoxOdometryModelVErr1.Text, NumberStyles.Float, CultureInfo.InvariantCulture, out vErr1) &&
                double.TryParse(textBoxOdometryModelVErr2.Text, NumberStyles.Float, CultureInfo.InvariantCulture, out vErr2) &&
                double.TryParse(textBoxOdometryModelRErr1.Text, NumberStyles.Float, CultureInfo.InvariantCulture, out rErr1) &&
                double.TryParse(textBoxOdometryModelRErr2.Text, NumberStyles.Float, CultureInfo.InvariantCulture, out rErr2)
                )
            {
                // send update odometry model message
                _eventsPort.Post(new OnUpdateOdometryModel(this, vErr1, vErr2, rErr1, rErr2));
            }
        }

        public void UpdateDriveData(drive.DriveDifferentialTwoWheelState data)
        {
            labelDebug.Text = data.LeftWheel.MotorState.Pose.Position.X + ", " +
                              data.LeftWheel.MotorState.Pose.Position.Z + ", " +
                              data.LeftWheel.MotorState.Pose.Orientation.W;
           // Console.Out.WriteLine("drive " + data.TimeStamp);
            // we have a state update in term of wheel positions
            // this can be converted to a pose estimate
        }
      
        #endregion

        #region Sensors Tab
        public void ReplaceSensors(IEnumerable<SensorState> sensors)
        {
            dataGridViewSensors.DataSource = null;
            dataGridViewSensors.DataSource = sensors;
            dataGridViewSensors.Refresh();

        }
        private void buttonNewSensor_Click(object sender, EventArgs e)
        {
            if (_eventsPort != null)
            {
                _eventsPort.Post(new OnNewSensor(this));
            }
        }

        public void UpdateLrfData(lrf.State data)
        {
           // Console.Out.WriteLine("lrf " + data.TimeStamp);
            // we have a spray of distance readings to add into a buffer for the next slam round
        }
        #endregion

        #region Sensor Types Tab

        public void ReplaceSensorTypes(List<SensorTypeState> sensorTypes)
        {
            // drop down list in sensors grid
            sensorTypeIdDataGridViewComboBoxColumn.DataSource = sensorTypes;
            sensorTypeIdDataGridViewComboBoxColumn.DisplayMember = "Name";
            sensorTypeIdDataGridViewComboBoxColumn.ValueMember = "Id";
            sensorTypeNameDataGridViewTextBoxColumn.ValueType = typeof(int);

            // main sensor type list
            dataGridViewSensorTypes.DataSource = null;
            dataGridViewSensorTypes.DataSource = sensorTypes;
            dataGridViewSensorTypes.Refresh();
        }

        private void buttonNewSensorType_Click(object sender, EventArgs e)
        {
            // CREATE NEW SESOR TYPE
            // tell the service to create a new one...
            if (_eventsPort != null)
            {
                _eventsPort.Post(new OnNewSensorType(this));
            }
        }
        #endregion

        #region Cone Models Tab

        public void ReplaceConeModels(IEnumerable<ConeModelState> coneModels)
        {
            // combo box in sensor type list
            coneModelIdDataGridViewComboBoxColumn.DataSource = coneModels;
            coneModelIdDataGridViewComboBoxColumn.DisplayMember = "Name";
            coneModelIdDataGridViewComboBoxColumn.ValueMember = "Id";
            coneModelIdDataGridViewComboBoxColumn.ValueType = typeof (int);

            // main cone model list
            dataGridViewConeModels.DataSource = null;
            dataGridViewConeModels.DataSource = coneModels;
            dataGridViewConeModels.Refresh();
        }
        private void buttonNewConeModel_Click(object sender, EventArgs e)
        {
            // tell the service to create a new one...
            if (_eventsPort != null)
            {
                _eventsPort.Post(new OnNewConeModel(this));
            }
        }

        #endregion

        #region Beam Models Tab

        public void ReplaceBeamModels(IEnumerable<BeamModelState> beamModels)
        {
            // combo box in sensor type list
            beamModelIdDataGridViewComboBoxColumn.DataSource = beamModels;
            beamModelIdDataGridViewComboBoxColumn.DisplayMember = "Name";
            beamModelIdDataGridViewComboBoxColumn.ValueMember = "Id";
            beamModelIdDataGridViewComboBoxColumn.ValueType = typeof(int);

            // main beam model list
            dataGridViewBeamModels.DataSource = null;
            dataGridViewBeamModels.DataSource = beamModels;
            dataGridViewBeamModels.Refresh();
        }
        private void buttonNewBeamModel_Click(object sender, EventArgs e)
        {
            // CREATE NEW BEAM MODEL
            // tell the service to create a new one...
            if (_eventsPort != null)
            {
                _eventsPort.Post(new OnNewBeamModel(this));
            }
        }
        #endregion

        #region GridSlam
        private void timerMapUpdate_Tick(object sender, EventArgs e)
        {
            // ask gridslam for the best map
            if (_eventsPort != null)
            {
                if (radioButtonMapBest.Checked)
                {
                    _eventsPort.Post(new OnRequestMapUpdate(this, MapType.BestMap));
                }
                else
                {
                    _eventsPort.Post(new OnRequestMapUpdate(this, MapType.RawMap));
                }
            }
        }

        public void UpdateMap(Bitmap b)
        {
            pictureBoxLocalMap.Image = b;
            pictureBoxLocalMap.Refresh();
        }
        #endregion

        private void buttonClearMap_Click(object sender, EventArgs e)
        {
            // send clear map message
            if (_eventsPort != null)
            {
                _eventsPort.Post(new OnClearMaps(this));
            }
        }

        private void dataGridViewBeamModels_RowEnter(object sender, DataGridViewCellEventArgs e)
        {
            // update beam model picture

            if (dataGridViewBeamModels.CurrentRow != null) {
            int beamModelRow = dataGridViewBeamModels.CurrentRow.Index;

            if (_eventsPort != null)
            {
                _eventsPort.Post(new OnRequestBeamModelImage(this,beamModelRow, pictureBoxBeamModel.Width, pictureBoxBeamModel.Height));
            }
            }
        }

        public void UpdateBeamModelImage(Image i)
        {
            pictureBoxBeamModel.Image = i;
            pictureBoxBeamModel.Refresh();
        }

        private void dataGridViewBeamModels_CellValueChanged(object sender, DataGridViewCellEventArgs e)
        {
            // beam model edit
            Console.Out.WriteLine("Beam cell edit");
        }
    }

    #region Form Event Operations and Types
    class GridSlamFormEvents : PortSet<
         OnLoad,
         OnClosed,
         OnNewConeModel,
         OnNewBeamModel,
         OnNewSensorType,
         OnNewSensor,
         OnRequestMapUpdate,
         OnRequestBeamModelImage,
         OnUpdateOdometryModel,
         OnClearMaps,
         OnConnect,
         OnConnectDrive,
         OnConnectLRF,
         OnDisconnectLRF
    > { }

    class GridSlamFormEvent
    {
        private GridSlamForm _gridSlamForm;

        public GridSlamForm GridSlamForm
        {
            get { return _gridSlamForm; }
            set { _gridSlamForm = value; }
        }

        public GridSlamFormEvent(GridSlamForm gridSlamForm)
        {
            _gridSlamForm = gridSlamForm;
        }
    }

    class OnLoad : GridSlamFormEvent
    {
        public OnLoad(GridSlamForm form)
            : base(form)
        {
        }
    }

    class OnClosed : GridSlamFormEvent
    {
        public OnClosed(GridSlamForm form)
            : base(form)
        {
        }
    }

    class OnConnect : GridSlamFormEvent
    {
        string _service;

        public string Service
        {
            get { return _service; }
            set { _service = value; }
        }

        public OnConnect(GridSlamForm form, string service)
            : base(form)
        {
            _service = service;
        }
    }

    /// <summary>
    /// Empty message saying that we need a new Cone Model
    /// </summary>
    class OnNewConeModel : GridSlamFormEvent
    {
        public OnNewConeModel(GridSlamForm form)
            : base(form)
        {
        }
    }

    /// <summary>
    /// Empty message saying that we need a new Beam Model
    /// </summary>
    class OnNewBeamModel : GridSlamFormEvent
    {
        public OnNewBeamModel(GridSlamForm form)
            : base(form)
        {
        }
    }

    class OnRequestBeamModelImage : GridSlamFormEvent
    {
        private int _row;

        public int Row
        {
            get { return _row; }
            set { _row = value;}
        }

        private int _width;

        public int Width
        {
            get { return _width; }
            set { _width = value; }
        }

        private int _height;

        public int Height
        {
            get { return _height; }
            set { _height = value; }
        }

        public OnRequestBeamModelImage(GridSlamForm form, int row, int width, int height)
            : base(form)
        {
            _row = row;
            _width = width;
            _height = height;
        }
    }


    /// <summary>
    /// Empty message saying that we need a new SensorType
    /// </summary>
    class OnNewSensorType : GridSlamFormEvent
    {
        public OnNewSensorType(GridSlamForm form)
            : base(form)
        {
        }
    }

    /// <summary>
    /// Empty message saying that we need a new Sensor
    /// </summary>
    class OnNewSensor : GridSlamFormEvent
    {
        public OnNewSensor(GridSlamForm form)
            : base(form)
        {
        }
    }


    /// <summary>
    /// Empty message saying that we would like to see a new best map
    /// </summary>
    class OnRequestMapUpdate : GridSlamFormEvent
    {
        MapType _mapType;

        public MapType MapType
        {
            get { return _mapType; }
            set { _mapType = value; }
        }
        public OnRequestMapUpdate(GridSlamForm form, MapType mapType)
            : base(form)
        {
            MapType = mapType;
        }
    }

    class OnClearMaps : GridSlamFormEvent
    {
        public OnClearMaps(GridSlamForm form)
            : base(form)
        {
        }
    }

    class OnUpdateOdometryModel : GridSlamFormEvent
    {
        double _vErr1;

        public double VErr1
        {
            get { return _vErr1; }
            set { _vErr1 = value; }
        }

        double _vErr2;

        public double VErr2
        {
            get { return _vErr2; }
            set { _vErr2 = value; }
        }

        double _rErr1;

        public double RErr1
        {
            get { return _rErr1; }
            set { _rErr1 = value; }
        }

        double _rErr2;

        public double RErr2
        {
            get { return _rErr2; }
            set { _rErr2 = value; }
        }

        public OnUpdateOdometryModel(GridSlamForm form, double vErr1, double vErr2, double rErr1, double rErr2)
            : base(form)
        {
            _vErr1 = vErr1;
            _vErr2 = vErr2;
            _rErr1 = rErr1;
            _rErr2 = rErr2;
        }
    }

    class OnConnectDrive : OnConnect
    {
        public OnConnectDrive(GridSlamForm form, string service)
            : base(form, service)
        {
        }
    }

    class OnConnectLRF : OnConnect
    {
        public OnConnectLRF(GridSlamForm form, string service)
            : base(form, service)
        {
        }
    }

    class OnDisconnectLRF : GridSlamFormEvent
    {
        public OnDisconnectLRF(GridSlamForm form)
            : base(form)
        {
        }
    }

    #endregion
}
